#include "Filter.h"

#define NUM 10


uint8_t num=0;
int sum=0;
int Filter_LeftArr[NUM];
int Filter_RightArr[NUM];
int Filter_LeftMAX=0;
int Filter_LeftMIN=0;
int Filter_RightMAX=0;
int Filter_RightMIN=0;
extern MotorRpm Motor_Rpm;

int Filter_Left_CurRpm(int Cur_Temp)
{
	sum=0;
	if (Cur_Temp>3000 || Cur_Temp<200) return Motor_Rpm.LeftExpRpm;
	Filter_LeftMAX=(Filter_LeftMAX>Cur_Temp) ? Filter_LeftMAX : Cur_Temp;
	Filter_LeftMIN=(Filter_LeftMIN<Cur_Temp) ? Filter_LeftMIN : Cur_Temp;
	num=(num==NUM)? 0 : (num+1);
	Filter_LeftArr[num]=Cur_Temp;
	for (int i=0;i<NUM;i++)
	{
		sum+=Filter_LeftArr[i];
	}
	return (sum-Filter_LeftMAX-Filter_LeftMIN)/(NUM-2); 
}


int Filter_Right_CurRpm(int Cur_Temp)
{
	sum=0;
	if (Cur_Temp>3000 || Cur_Temp<200) return Motor_Rpm.RightExpRpm;
	Filter_RightMAX=(Filter_RightMAX>Cur_Temp) ? Filter_RightMAX : Cur_Temp;
	Filter_RightMIN=(Filter_RightMIN<Cur_Temp) ? Filter_RightMIN : Cur_Temp;
	num=(num==NUM)? 0 : (num+1);
	Filter_RightArr[num]=Cur_Temp;
	for (int i=0;i<NUM;i++)
	{
		sum+=Filter_RightArr[i];
	}
	return (sum-Filter_RightMAX-Filter_RightMIN)/(NUM-2); 
}

void Filter_Param_Set(void)
{
	for (int i=0;i<NUM;i++)
	{
		Filter_RightArr[i]=Motor_Rpm.RightExpRpm;
		Filter_LeftArr[i]=Motor_Rpm.LeftExpRpm;
	}
	
	Filter_LeftMAX=Motor_Rpm.LeftExpRpm;
	Filter_LeftMIN=Motor_Rpm.LeftExpRpm;
	Filter_RightMAX=Motor_Rpm.RightExpRpm;
	Filter_RightMIN=Motor_Rpm.RightExpRpm;
}


